#!/usr/bin/env python
 
import tkinter as tk
import rospy
from agv_msgs.msg import ReadInPutsNew, PairTypeInt, AvoidObsta, AvoidObstaInfo
from std_msgs.msg import Bool
import threading
 
class ButtonApp:
    def __init__(self, root):
        self.root = root
        self.root.title("IO Button App")
        self.root.geometry('240x480+10+10') 

        self.button_bg_color = "lightblue"
        self.button_fg_color = "darkblue"
        self.button_press_color = "green"

        self.absavoid_message = AvoidObstaInfo()
        self.absavoid_message.scenes_name = 'fake_obsta_msg'
        avoid_msg = AvoidObsta()
        avoid_msg.slow = 0
        avoid_msg.stop = 0
        self.absavoid_message.avoid_msg.append(avoid_msg)

        self.key_message = ReadInPutsNew()
        self.estop = PairTypeInt()
        self.bumper = PairTypeInt()
        self.reset = PairTypeInt()
        self.stop = PairTypeInt()
        self.release = PairTypeInt()
        self.mode = PairTypeInt()
        self.estop.key = "emergencyButton"
        self.bumper.key = "bumper"
        self.reset.key = "resetButton"
        self.stop.key = "stopButton"
        self.release.key = "releaseButton"
        self.mode.key = "modeButton"
        
        self.key_message.array.append(self.estop)
        self.key_message.array.append(self.bumper)
        self.key_message.array.append(self.reset)
        self.key_message.array.append(self.stop)
        self.key_message.array.append(self.release)
        self.key_message.array.append(self.mode)

        self.create_button("estop", self.estop, self.on_estop_press, self.on_estop_release)
        self.create_button("bumper", self.bumper, self.on_bumper_press, self.on_bumper_release)
        self.create_button("reset", self.reset, self.on_reset_press, self.on_reset_release)
        self.create_button("stop", self.stop, self.on_stop_press, self.on_stop_release)
        self.create_button("mode", self.mode, self.on_mode_press, self.on_mode_release)  # Existing mode button
        self.create_button("absslow", self.absavoid_message, self.on_absslow_press, self.on_absslow_release)
        self.create_button("absstop", self.absavoid_message, self.on_absstop_press, self.on_absstop_release)
        self.lock = threading.Lock()
 
        # Initialize ROS node and publisher
        rclpy.init()
        node = rclpy.create_node('button_app', anonymous=True)
        self.io_pub = node.create_publisher(ReadInPutsNew, queue_size=10, '/readInputNew')
        self.avoid_obsta_pub = node.create_publisher(AvoidObstaInfo, queue_size=10, '/agv_obstacle_level')

        self.periodic_publisher_thread = threading.Thread(target=self.periodic_publisher, daemon=True)
        self.periodic_publisher_thread.start()

    def create_button(self, button_text, button_var, press_callback, release_callback):
        button = tk.Button(self.root, text=button_text, width=10, bg=self.button_bg_color, fg=self.button_fg_color)
        button.bind("<Button-1>", press_callback)
        button.bind("<ButtonRelease-1>", release_callback)
        button.pack(pady=10)
        setattr(self, f"{button_text.lower()}_button", button)  # Store button reference
 
    def on_estop_press(self, event=None):
        with self.lock:
            rospy.loginfo("estopButton clicked")
            if (self.estop.value == 1):
                self.estop.value = 0
                getattr(self, "estop_button").configure(bg=self.button_bg_color)
            else:
                self.estop.value = 1
                getattr(self, "estop_button").configure(bg=self.button_press_color)
 
    def on_estop_release(self, event):
        with self.lock:
            rospy.loginfo("estopButton released")
            # self.estop.value = 0
 
    def on_bumper_press(self, event=None):
        with self.lock:
            rospy.loginfo("bumperButton clicked")
            if (self.bumper.value == 1):
                self.bumper.value = 0
                getattr(self, "bumper_button").configure(bg=self.button_bg_color)
            else:
                self.bumper.value = 1
                getattr(self, "bumper_button").configure(bg=self.button_press_color)
 
    def on_bumper_release(self, event):
        with self.lock:
            rospy.loginfo("bumperButton released")
            # self.bumper.value = 0
 
    def on_reset_press(self, event=None):
        with self.lock:
            rospy.loginfo("resetButton clicked")
            self.reset.value = 1
 
    def on_reset_release(self, event):
        with self.lock:
            rospy.loginfo("resetButton released")
            self.reset.value = 0
 
    def on_stop_press(self, event=None):
        with self.lock:
            rospy.loginfo("stopButton clicked")
            self.stop.value = 1
 
    def on_stop_release(self, event):
        with self.lock:
            rospy.loginfo("stopButton released")
            self.stop.value = 0
 
    def on_mode_press(self, event=None):
        with self.lock:
            rospy.loginfo("modeButton clicked")
            self.mode.value = 1
 
    def on_mode_release(self, event):
        with self.lock:
            rospy.loginfo("modeButton released")
            self.mode.value = 0

    def on_absslow_press(self, event=None):
        with self.lock:
            rospy.loginfo("absslow clicked")
            if (self.absavoid_message.avoid_msg[0].slow):
                self.absavoid_message.avoid_msg[0].slow = 0
                getattr(self, "absslow_button").configure(bg=self.button_bg_color)
            else:
                self.absavoid_message.avoid_msg[0].slow = 1
                getattr(self, "absslow_button").configure(bg=self.button_press_color)

    def on_absslow_release(self, event):
        with self.lock:
            rospy.loginfo("absslow released")
            # self.absavoid_message.avoid_msg[0].slow = 0

    def on_absstop_press(self, event=None):
        with self.lock:
            rospy.loginfo("absstop clicked")
            if (self.absavoid_message.avoid_msg[0].stop):
                self.absavoid_message.avoid_msg[0].stop = 0
                getattr(self, "absstop_button").configure(bg=self.button_bg_color)
            else:
                self.absavoid_message.avoid_msg[0].stop = 1
                getattr(self, "absstop_button").configure(bg=self.button_press_color)

    def on_absstop_release(self, event):
        with self.lock:
            rospy.loginfo("absstop released")
            # self.absavoid_message.avoid_msg[0].stop = 0

    def periodic_publisher(self):
        rate = rospy.Rate(10)  # 10Hz
        while not rospy.is_shutdown():
            with self.lock:
                # Create a new message for each publish (to avoid modification issues in multithreading)
                periodic_message = ReadInPutsNew()
                for item in self.key_message.array:
                    periodic_message.array.append(PairTypeInt(key=item.key, value=item.value))
                self.io_pub.publish(periodic_message)
                time_now = rospy.Time.now()
                self.absavoid_message.time.data.secs = int(time_now.to_sec())
                self.avoid_obsta_pub.publish(self.absavoid_message)
            rate.sleep()
 
if __name__ == "__main__":
    root = tk.Tk()
    app = ButtonApp(root)
    root.mainloop()